#ifndef __POSE_ESTIMATOR_H
#define __POSE_ESTIMATOR_H

#include "ATANCamera.h"
#include <TooN/TooN.h>
#include <TooN/se3.h>

struct ThreeViewHypothesis 
{
	friend bool operator < (const ThreeViewHypothesis &h1, const ThreeViewHypothesis &h2)	{ return h1.dError < h2.dError; };

  /*void computeTrifocalTensor()
  {
    m33Tf0 = (se3T10.get_rotation().get_matrix().slice(0,0,4,1) * se3T20.get_translation().as_row()) - 
              (se3T10.get_translation().as_col() * se3T20.get_rotation().get_matrix().slice(0,0,4,1).T());

    m33Tf1 = (se3T10.get_rotation().get_matrix().slice(0,1,4,1) * se3T20.get_translation().as_row()) - 
              (se3T10.get_translation().as_col() * se3T20.get_rotation().get_matrix().slice(0,1,4,1).T());

    m33Tf2 = (se3T10.get_rotation().get_matrix().slice(0,2,4,1) * se3T20.get_translation().as_row()) - 
              (se3T10.get_translation().as_col() * se3T20.get_rotation().get_matrix().slice(0,2,4,1).T());
  };*/

  void computeTrifocalTensor()
  {
    const Vector<3> &v3T20 = se3T20.get_translation();
    const Vector<3> &v3T10 = se3T10.get_translation();
    const Matrix<3,3> &m33R20 = se3T20.get_rotation().get_matrix();
    const Matrix<3,3> &m33R10 = se3T10.get_rotation().get_matrix();

    m33Tf0(0,0) = m33R10(0,0)*v3T20[0] - v3T10[0]*m33R20(0,0);
    m33Tf1(0,0) = m33R10(0,0)*v3T20[1] - v3T10[0]*m33R20(1,0);
    m33Tf2(0,0) = m33R10(0,0)*v3T20[2] - v3T10[0]*m33R20(2,0);
    m33Tf0(0,1) = m33R10(1,0)*v3T20[0] - v3T10[1]*m33R20(0,0);
    m33Tf1(0,1) = m33R10(1,0)*v3T20[1] - v3T10[1]*m33R20(1,0);
    m33Tf2(0,1) = m33R10(1,0)*v3T20[2] - v3T10[1]*m33R20(2,0);
    m33Tf0(0,2) = m33R10(2,0)*v3T20[0] - v3T10[2]*m33R20(0,0);
    m33Tf1(0,2) = m33R10(2,0)*v3T20[1] - v3T10[2]*m33R20(1,0);
    m33Tf2(0,2) = m33R10(2,0)*v3T20[2] - v3T10[2]*m33R20(2,0);

    m33Tf0(1,0) = m33R10(0,1)*v3T20[0] - v3T10[0]*m33R20(0,1);
    m33Tf1(1,0) = m33R10(0,1)*v3T20[1] - v3T10[0]*m33R20(1,1);
    m33Tf2(1,0) = m33R10(0,1)*v3T20[2] - v3T10[0]*m33R20(2,1);
    m33Tf0(1,1) = m33R10(1,1)*v3T20[0] - v3T10[1]*m33R20(0,1);
    m33Tf1(1,1) = m33R10(1,1)*v3T20[1] - v3T10[1]*m33R20(1,1);
    m33Tf2(1,1) = m33R10(1,1)*v3T20[2] - v3T10[1]*m33R20(2,1);
    m33Tf0(1,2) = m33R10(2,1)*v3T20[0] - v3T10[2]*m33R20(0,1);
    m33Tf1(1,2) = m33R10(2,1)*v3T20[1] - v3T10[2]*m33R20(1,1);
    m33Tf2(1,2) = m33R10(2,1)*v3T20[2] - v3T10[2]*m33R20(2,1);

    m33Tf0(2,0) = m33R10(0,2)*v3T20[0] - v3T10[0]*m33R20(0,2);
    m33Tf1(2,0) = m33R10(0,2)*v3T20[1] - v3T10[0]*m33R20(1,2);
    m33Tf2(2,0) = m33R10(0,2)*v3T20[2] - v3T10[0]*m33R20(2,2);
    m33Tf0(2,1) = m33R10(1,2)*v3T20[0] - v3T10[1]*m33R20(0,2);
    m33Tf1(2,1) = m33R10(1,2)*v3T20[1] - v3T10[1]*m33R20(1,2);
    m33Tf2(2,1) = m33R10(1,2)*v3T20[2] - v3T10[1]*m33R20(2,2);
    m33Tf0(2,2) = m33R10(2,2)*v3T20[0] - v3T10[2]*m33R20(0,2);
    m33Tf1(2,2) = m33R10(2,2)*v3T20[1] - v3T10[2]*m33R20(1,2);
    m33Tf2(2,2) = m33R10(2,2)*v3T20[2] - v3T10[2]*m33R20(2,2);
  }


  TooN::Matrix<3,3> m33E20;
  TooN::SE3<> se3T20; 
  TooN::SE3<> se3T10;
  TooN::Matrix<3,3> m33Tf2;
  TooN::Matrix<3,3> m33Tf1; 
  TooN::Matrix<3,3> m33Tf0;
	double dError;
  int nErrorPoints;
};

struct PoseHypothesis
{
	friend bool operator < (const PoseHypothesis &h1, const PoseHypothesis &h2)	{ return h1.dError < h2.dError; };

  TooN::SE3<> se3CfromW;
  double dError;
  int nErrorPoints;
};

struct InitialPoseHypothesis
{
  TooN::SE3<> se3WfromC;
  double dMaxPositionError;
  double dMaxRotationError;
};

struct Observation
{
  TooN::Vector<2> v2ImagePos;
  TooN::Vector<3> v3Ray;
  TooN::Vector<3> v3WorldPos;
  TooN::Vector<3> v3CamPos;

  void SetAssigned()
  {
    ctype = ASSIGNED;
  }

  bool IsAssigned()
  {
    return (ctype == ASSIGNED);
  }

  void SetUnAssigned()
  {
    ctype = UNASSIGNED;
  }

  bool IsUnAssigned()
  {
    return (ctype == UNASSIGNED);
  }

  void SetNoise()
  {
    ctype = NOISE;
  }

  bool IsNoise()
  {
    return (ctype == NOISE);
  }

private:
  enum ClusterType { UNASSIGNED, NOISE, ASSIGNED };
  ClusterType ctype;
};

/*!
  This class offers a simple interface for absolute/relative pose estimation.
  5 point relative pose partially based on Nister (2004) "An efficient solution to the five point relative pose problem".
  3 point absolute pose based on Finsterwalder's Solution. See Haralick (1994) "Review and Analysis of Solutions of the Three Point Perspective
  Pose Estimation Problem".
 */

class PoseEstimator
{
public:
  PoseEstimator(const ATANCamera &camera);
  ~PoseEstimator() {};

	bool threePointAbsolute(const std::vector<Observation*> &vObservations, PoseHypothesis &hyp, bool bRelativeToCamPos = false);
  bool threePointAbsoluteRANSAC(const unsigned int nNumHypo, const unsigned int nNumBlocks, const unsigned int nGroupsPerBlock, const unsigned int nGroupSize, const std::vector<Observation*> &vObservations, PoseHypothesis &bestPose, InitialPoseHypothesis *pInitialPose = NULL);

  bool threeViewRelative(const std::vector<Observation*> &vObservations0, const std::vector<Observation*> &vObservations1, const std::vector<Observation*> &vObservations2, ThreeViewHypothesis &hyp);
	bool threeViewRelativeRANSAC(const unsigned int nNumHypo, const unsigned int nNumBlocks, const unsigned int nBlockSize, const std::vector<Observation*> &vObservations0, const std::vector<Observation*> &vObservations1, const std::vector<Observation*> &vObservations2, ThreeViewHypothesis &bestHypothesis);

  void runtest();

private:
  ATANCamera mCamera;

  Vector<2> addPixelNoise(const Vector<2>& imframe);
 // void fivePointRelative(const std::vector<Observation*> &vObservations0, const std::vector<Observation*> &vObservations1, TooN::Matrix<9,10> &mv9E, int &nNumSolutions);
  double computeTrifocalSampsonError(const ThreeViewHypothesis &hyp, const Observation* pObs0, const Observation* pObs1, const Observation* pObs2);
  Vector<3> ReprojectPoint(SE3<> se3AfromB, const Vector<2> &v2A, const Vector<2> &v2B);
  Matrix<3,3> matrixCrossProduct(Vector<3> v3A);
  void evecToMat(const Matrix<9,1> &m91E, TooN::Matrix<3,3> &m33E);
  void extractRt(const TooN::Matrix<3,3> &m33E10, Observation* pObs0, const Observation* pObs1, TooN::SE3<> &se3T10);
  inline double sign(const double value) { return (value >= 0) ? 1 : -1; }
  void solveCubic(double (&coeffs)[4], double (&roots)[3]);
  double determinant(const TooN::SO3<> &so3);
  double caucyError(const TooN::SE3<> &se3CfromW, const std::vector<Observation*> &obs);
  double squaredProjectionError(const SE3<> &se3CfromW, const Vector<3> &v3WorldPos, const Vector<2> &v2ImagePos);
};

#endif
